#include "MyQueue.h"
#include <stdio.h>
#include "FreeRTOS.h"
#include <semphr.h>
#include "CmdHandler.h"
struct CmdQueue g_cmdQueue;
SemaphoreHandle_t cmd_queue_mutex;
int g_queue_inited = 0;
void MyQueueInit(){
	printf("Queue init.\r\n");
	cmd_queue_mutex = xSemaphoreCreateBinary();
	for(int i=0; i<QUEUE_ITEM_CNT; i++){
		g_cmdQueue.CmdItems[i].data = (unsigned char*)pvPortMalloc(CMD_ITEM_LEN);
	}
	g_queue_inited = 1;
	xSemaphoreGive(cmd_queue_mutex);
}

int GetMyQueueSize(){
	unsigned char cnt = 0;
	if(xSemaphoreTake(cmd_queue_mutex, portMAX_DELAY) == pdTRUE){
		for(int i=0; i<QUEUE_ITEM_CNT; i++){
			if(g_cmdQueue.CmdItems[i].len > 0){
				cnt ++;
			}
		}
	}
	xSemaphoreGive(cmd_queue_mutex);
	return cnt;
}

unsigned char isCmdQueueFull(){
	return (GetMyQueueSize() == QUEUE_ITEM_CNT);
}

int MyQueuePush(unsigned char* data, unsigned int dataLen, unsigned int cmdId, unsigned char needReply){
	printf("Push data to queue.\r\n");
	if(g_queue_inited == 0){
		printf("Error not inited.\r\n");
		return 0;
	}
	if(isCmdQueueFull()){
		printf("Error:CmdQueue is full.\r\n");
		return 0;
	}
	if(dataLen > CMD_ITEM_LEN){
		printf("Error:CmdData larger than Item buffer.\r\n");
		return 0;
	}
	if(dataLen <=0){
		printf("Error:Push Cmd ERROR: len:%d\r\n",dataLen);
		return 0;
	}
	int result = 0;
	if(xSemaphoreTake(cmd_queue_mutex, portMAX_DELAY) == pdTRUE){
		for(int i=0; i<QUEUE_ITEM_CNT; i++){
			if((g_cmdQueue.CmdItems[i].len <= 0) && (NULL != g_cmdQueue.CmdItems[i].data)){
				g_cmdQueue.CmdItems[i].cmdId = cmdId;
				g_cmdQueue.CmdItems[i].needReply = needReply;
				g_cmdQueue.CmdItems[i].len = dataLen;
				g_cmdQueue.CmdItems[i].timeStamp = ReadDs3231TimeStamp();
				memcpy(g_cmdQueue.CmdItems[i].data,data,dataLen);
				result = 1;
				goto exitMyQueuePush;
			}
		}
	}
exitMyQueuePush:	
	xSemaphoreGive(cmd_queue_mutex);
	printf("Queue size:%d\r\n",GetMyQueueSize());
	return result;
}

int MyQueuePop(unsigned char* buffer, int bufferLen, unsigned int* cmdId, unsigned char* needReply){
	int ret = 0;
	//printf("Pop DATA from QUEUE.\r\n");
	if(xSemaphoreTake(cmd_queue_mutex, portMAX_DELAY) == pdTRUE){
		for(int i=0; i<QUEUE_ITEM_CNT;i++){
			if(g_cmdQueue.CmdItems[i].len > 0){
				if(bufferLen < g_cmdQueue.CmdItems[i].len){
					printf("myQueue Pop Error: buff not enough.\r\n");
					ret = g_cmdQueue.CmdItems[i].len * -1;
					goto exitMyQueuePop;
				}
				else{
					
					memcpy(buffer,g_cmdQueue.CmdItems[i].data,g_cmdQueue.CmdItems[i].len);
					ret = g_cmdQueue.CmdItems[i].len;
					g_cmdQueue.CmdItems[i].len = 0;
					g_cmdQueue.CmdItems[i].timeStamp = 0;
					*cmdId = g_cmdQueue.CmdItems[i].cmdId;
					*needReply = g_cmdQueue.CmdItems[i].needReply;
					printf("Pop DATA from QUEUE id:%d\r\n.\r\n",*cmdId);
					goto exitMyQueuePop;
				}
			}
		}
	}
exitMyQueuePop:
	xSemaphoreGive(cmd_queue_mutex);
	return ret;
}

void MyQueueDestroy(){
	printf("Queue destroy.\r\n");
	if(xSemaphoreTake(cmd_queue_mutex, portMAX_DELAY) == pdTRUE){
		for(int i=0; i<QUEUE_ITEM_CNT;i++){
			g_cmdQueue.CmdItems[i].len = 0;
		}
	}
	xSemaphoreGive(cmd_queue_mutex);
}
